#include "DriverModel.h"
#include "DriverData.h"

namespace Driver {
// helper type for the visitor #4
template<class... Ts>
struct overloaded : Ts... { using Ts::operator()...; };
// explicit deduction guide (not needed as of C++20)
template<class... Ts>
overloaded(Ts...) -> overloaded<Ts...>;


using namespace sml;

DriverModel::DriverModel(SlavePDOS *pdos)
    : driver_data_{pdos}
    , sm_{driver_data_}
    {
        std::cout << "DriverModel::DriverModel() rx_pdo:" << &(pdos->rx_pdos) << std::endl;
    }

DriverModel::~DriverModel() = default;

void DriverModel::TimeTick() {
    sm_.process_event(TickEvent{});
    SwitchToTargetState();
}

void DriverModel::FaultReset() {
    sm_.process_event(FaultResetEvent{});
}

void DriverModel::ShutDown() {
    sm_.process_event(ShutdownEvent{});
}

void DriverModel::SwitchOn() {

    sm_.process_event(SwitchOnEvent{});
}

void DriverModel::DisableOperation() {
    sm_.process_event(DisableOperationEvent{});
}

void DriverModel::EnableOperation() {
    sm_.process_event(EnableOperationEvent{});
}

void DriverModel::DisableVoltage() {
    sm_.process_event(DisableVoltageEvent{});
}

void DriverModel::QuickStop() {
    sm_.process_event(QuickStopEvent{});
}

// bool DriverModel::IsNotReadyToSwitchOn() {
//     return sm_.is(sml::state<TransitionTable::NotReadyToSwitchOnState>);
// }

bool DriverModel::IsNotReadyToSwitchOn() {
    return driver_data_.IsNotReadyToSwitchOn();
}

bool DriverModel::IsNormal() {
  return IsSwitchOnDisabled() || IsReadyToSwitchOn() || IsSwitchedOn() ||
         IsOperationEnabled() || IsQuickStopActive();
}

bool DriverModel::IsFault() {
    return driver_data_.IsErrored();
}

bool DriverModel::IsFaultReactionActive() {
    return driver_data_.IsFaultReactionActive();
}

bool DriverModel::IsSwitchOnDisabled() {
    return driver_data_.IsSwitchOnDisabled();
}

bool DriverModel::IsReadyToSwitchOn() {
  return driver_data_.IsReadyToSwitchOn();
}

bool DriverModel::IsSwitchedOn() {
    return driver_data_.IsSwitchedOn();
}

bool DriverModel::IsOperationEnabled() {
    return driver_data_.IsOperationEnabled();
}

bool DriverModel::IsQuickStopActive() {
    return driver_data_.IsQuickStoped();
}

bool DriverModel::IsStandStill() {
    return sm_.is<decltype(sml::state<OperationEnabledState>)>(state<OperationEnabledState::StandStillState>); ;
}

bool DriverModel::IsMoving() {
    return sm_.is<decltype(sml::state<OperationEnabledState>)>(state<OperationEnabledState::MovingState>);
}

bool DriverModel::IsHome() {
    return IsHomeDisable()
    || IsReadyToHome()
    || IsHoming()
    || IsHomeFinished()
    || IsHomeHalted()
    || IsSwitchToHomeError()
    || IsHomeErrored();
}

bool DriverModel::IsHomeDisable() {
    return sm_.is<decltype(sml::state<HomeState>)>(state<HomeState::HomeDisabledState>);
}

bool DriverModel::IsReadyToHome() {
    return sm_.is<decltype(sml::state<HomeState>)>(state<HomeState::ReadyToHomeState>);
}

bool DriverModel::IsHoming() {
    return sm_.is<decltype(sml::state<HomeState>)>(state<HomeState::HomingState>);
}

bool DriverModel::IsHomeFinished() {
    return sm_.is<decltype(sml::state<HomeState>)>(state<HomeState::HomeFinishedState>);
}

bool DriverModel::IsHomeHalted() {
    return sm_.is<decltype(sml::state<HomeState>)>(state<HomeState::HomeHaltedState>);
}

bool DriverModel::IsSwitchToHomeError() {
    return sm_.is<decltype(sml::state<HomeState>)>(state<HomeState::SwitchToHomeErrorState>);
}

bool DriverModel::IsHomeErrored() {
    return sm_.is<decltype(sml::state<HomeState>)>(state<HomeState::HomeErrorState>);
}

void DriverModel::PowerOn() {
    target_state_ = sml::state<OperationEnabledState>;
}

void DriverModel::PowerOff() {
    target_state_ = sml::state<NormalState::SwitchOnDisabledState>;
}

void DriverModel::Home() { sm_.process_event(HomeEvent{}); }

void DriverModel::CancelHome() {
    sm_.process_event(HaltHomeEvent{});
}

void DriverModel::Move() {
    sm_.process_event(MoveEvent{});
}

void DriverModel::Stop() {
    sm_.process_event(StopEvent{});
}

void DriverModel::Faster() {
    sm_.process_event(FasterEvent{});
}

void DriverModel::Slower() {
    sm_.process_event(SlowerEvent{});
}

void DriverModel::HaltHome() {
    sm_.process_event(HaltHomeEvent{});
}

void DriverModel::SwitchToTargetState() {
    std::visit(overloaded{
        [] (std::monostate) {},
        [&](decltype(sml::state<OperationEnabledState>)) {
            if (IsSwitchOnDisabled()) {
                std::cout << "IsSwitchOnDisabled" << std::endl;
                ShutDown();
            } else if (IsReadyToSwitchOn()) {
                std::cout << "IsReadyToSwitchOn" << std::endl;
                SwitchOn();
            } else if (IsSwitchedOn()) {
                std::cout << "IsSwitchedOn" << std::endl;
                EnableOperation();
            } else if (IsOperationEnabled()) {
                target_state_ = std::monostate{};
            }
        },
        [&](decltype(sml::state<NormalState::SwitchOnDisabledState>)) {
            if (IsOperationEnabled()) {
                std::cout << "IsOperationEnabled" << std::endl;
                DisableOperation();
            } else if (IsSwitchedOn()) {
                std::cout << "IsSwitchedOn" << std::endl;
                ShutDown();
            } else if (IsReadyToSwitchOn()) {
                std::cout << "IsReadyToSwitchOn" << std::endl;
                DisableVoltage();
            } else if (IsSwitchOnDisabled()) {
                std::cout << "IsSwitchOnDisabled" << std::endl;
                target_state_ = std::monostate{};
            }
        }
    }, target_state_);
}

}  // namespace Driver